// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceTaskDescriptionGenerator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Top level class to generate pick and place sub task descriptions. 
    /// A sub task description contains enough information for execution by the pick and place
    /// FSM engine
    /// </summary>
    public class PicknPlaceTaskDescriptionGenerator
    {
        /// <summary>
        /// The pick and place arm / hand gripper hardware
        /// </summary>
        private IPicknPlaceHardwareGrip armHandGripper;

        /// <summary>
        /// Default timeout at the end of each sub task step
        /// </summary>
        private int defaultTimeOutmSec;

        /// <summary>
        /// Initializes a new instance of the <see cref="PicknPlaceTaskDescriptionGenerator" /> class
        /// </summary>
        /// <param name="armHandGripper">The hardware specific armGripper generator</param>
        /// <param name="defaultTimeOutmSec">Default time out in mSec at end of each sub task step</param>
        public PicknPlaceTaskDescriptionGenerator(IPicknPlaceHardwareGrip armHandGripper, int defaultTimeOutmSec)
        {
            this.armHandGripper = armHandGripper;
            this.defaultTimeOutmSec = defaultTimeOutmSec;
        }

        /// <summary>
        /// Generic enumeration of all move and grasp sub tasks used in pick and place
        /// </summary>
        protected enum SubTaskTypesEnum
        {
            /// <summary>
            /// Pose adjustment in current or start position
            /// </summary>
            Current,

            /// <summary>
            /// Lift position above pre grip
            /// </summary>
            PreGripLift,

            /// <summary>
            /// pre grip
            /// </summary>
            PreGrip,

            /// <summary>
            /// Grip position for object
            /// </summary>
            Grip,

            /// <summary>
            /// Gripper action at grip position
            /// </summary>
            GripGripperAction,

            /// <summary>
            /// Lift position at the grasp position
            /// </summary>
            GripLift,

            /// <summary>
            /// Retract action
            /// </summary>
            Retract
        }

        /// <summary>
        /// Gets a list of move and grasp step descriptions that when executed implements a pick and move sub task
        /// </summary>
        /// <param name="manipulatedObject">Object to be picked</param>
        /// <param name="startPose">Position at start of the task. Enables re-orientation before a move</param>
        /// <param name="objectPose">Pose of the object to be grasped (example pick or place location)</param>
        /// <param name="movePose">Pose of object when grasped (example move to location or move from location)</param>
        /// <param name="liftHeight">Height object should be lifted in the pick action</param>
        /// <returns>List of step descriptions</returns>
        /// <exception cref="NoGripAvailableException">Thrown if request cannot be fulfilled and no subtasks are added to the list</exception>
        public List<MoveAndGraspTaskDescription> GetStepDescriptionsForPickTask(
            PhysicalObjectDescription manipulatedObject,
            Pose startPose,
            Pose objectPose,
            Pose movePose,
            double liftHeight)
        {
            Dictionary<SubTaskTypesEnum, PoseAndGrip> td = this.GetStepDescriptionsForAnyGrip(manipulatedObject, startPose, objectPose, movePose, liftHeight);
            List<MoveAndGraspTaskDescription> taskList = new List<MoveAndGraspTaskDescription>();
            this.AppendPickStepDescriptions(taskList, td, manipulatedObject);
            return taskList;
        }

        /// <summary>
        /// Gets a list of move and grasp step descriptions that when executed implement a place and move sub task
        /// </summary>
        /// <param name="manipulatedObject">Object to place</param>
        /// <param name="gripStyle">Current grip style</param>
        /// <param name="startPose">Position at start of the task. Enables re-orientation before a move</param>
        /// <param name="placePose">Object pose at placed location</param>
        /// <param name="retractPose">Pose of end effector after retraction</param>
        /// <param name="liftHeight">Height object should be lifted in the pick action</param>
        /// <param name="allowGripExchange">If true allows a grip exchange action to occur when the place cannot occur</param>
        /// <param name="gripExchangePose">Pose for grip exchange</param>
        /// <returns>The updated task list</returns>
        /// <exception cref="NoGripAvailableException">Thrown if request cannot be fulfilled and no subtasks are added to the list</exception>
        public List<MoveAndGraspTaskDescription> GetStepDescriptionsForPlaceTask(
            PhysicalObjectDescription manipulatedObject,
            GripStyle gripStyle,
            Pose startPose,
            Pose placePose,
            Pose retractPose,
            double liftHeight,
            bool allowGripExchange,
            Pose gripExchangePose)
        {
            bool retryWithGripExchange = false;
            List<MoveAndGraspTaskDescription> taskList = new List<MoveAndGraspTaskDescription>();

            try
            {
                Dictionary<SubTaskTypesEnum, PoseAndGrip> td = this.GetStepDescriptionsForGrip(manipulatedObject, gripStyle, startPose, placePose, retractPose, liftHeight);
                this.AppendPlaceStepDescriptions(taskList, td, manipulatedObject);
            }
            catch (NoGripAvailableException)
            {
                // Hardware cannot do the place with the grip style
                retryWithGripExchange = allowGripExchange;
                if (false == retryWithGripExchange)
                {
                    throw new NoGripAvailableException(string.Format(
                        "Cannot find a suitable grip for {0} at location {1} {2} {3} GripStyle {4}",
                        manipulatedObject.ToString(), 
                        placePose.Position.X, 
                        placePose.Position.Y, 
                        placePose.Position.Z,
                        gripStyle.ToString()));
                }
            }

            if (false == retryWithGripExchange)
            {
                return taskList;
            }

            // Invoke grip exchange
            Dictionary<SubTaskTypesEnum, PoseAndGrip> placeWithNewGripTaskDict = this.GetStepDescriptionsForAnyGrip(manipulatedObject, startPose, placePose, retractPose, liftHeight);
            if (null != placeWithNewGripTaskDict && placeWithNewGripTaskDict.ContainsKey(SubTaskTypesEnum.Grip))
            {
                GripStyle newGripStyle = placeWithNewGripTaskDict[SubTaskTypesEnum.Grip].GripperStyle;
                Dictionary<SubTaskTypesEnum, PoseAndGrip> placeExchangeTaskDict = this.GetStepDescriptionsForGrip(manipulatedObject, gripStyle, startPose, gripExchangePose, retractPose, liftHeight * 1.05);
                Dictionary<SubTaskTypesEnum, PoseAndGrip> pickExchangeTaskDict = this.GetStepDescriptionsForGrip(manipulatedObject, newGripStyle, retractPose, gripExchangePose, retractPose, liftHeight * 1.05);

                taskList.Clear();
                this.AppendGripExchangeStepDescriptions(taskList, placeExchangeTaskDict, pickExchangeTaskDict, manipulatedObject);
                this.AppendPlaceStepDescriptions(taskList, placeWithNewGripTaskDict, manipulatedObject, PicknPlaceStatusEnum.PlaceInProgress, true);
                return taskList;
            }
            else
            {
                throw new NoGripAvailableException(string.Format(
                    "Cannot find a suitable grip for {0} at location {1} {2} {3} GripStyle {4}",
                    manipulatedObject.ToString(),
                    placePose.Position.X,
                    placePose.Position.Y,
                    placePose.Position.Z,
                    gripStyle.ToString()));
            }
        }

        /// <summary>
        /// Construct a pre Grip description for picking an object giving its grip configuration
        /// </summary>
        /// <param name="grip">The Grasp description</param>
        /// <returns>Pre grip description</returns>
        internal GripState PreGripFromGrip(GripState grip)
        {
            Vector4 preGripPalmPlaneNormal = this.armHandGripper.PreGripPlane(grip.PalmPlane);
            Vector3 preGripCenter = this.armHandGripper.PreGripCenter(grip);

            GripState preGrip = new GripState(grip.GripStyle, preGripCenter, preGripPalmPlaneNormal, grip.GripCenterAxis);
            return preGrip;
        }

        /// <summary>
        /// Returns an arm pose corresponding to a grip. The position portion of the pose is set to NaN
        /// if the hardware cannot support the grip
        /// </summary>
        /// <param name="grip">Grip description</param>
        /// <param name="surfaceOffset">Surface offset</param>
        /// <param name="score">Output score measure of how far the pose is to limits. Bigger is better</param>
        /// <returns>arm pose</returns>
        /// <exception cref="ArgumentException">Hardware cannot execute the grip</exception>
        internal Pose ArmPoseFromGrip(GripState grip, double surfaceOffset, out double score)
        {
            Pose armPose = this.armHandGripper.ArmPoseFromGrip(grip, surfaceOffset, out score);
            return armPose;
        }

        /// <summary>
        /// Construct a grip description for picking an object from a location
        /// </summary>
        /// <param name="objectGrip">Manipulated object interface</param>
        /// <param name="objectPose">Object's pose</param>
        /// <param name="gripStyle">Specifies the grip type to use</param>
        /// <returns>Grip description</returns>
        internal GripState GripFromPickAndGripStyle(IPicknPlaceObjectGrip objectGrip, Pose objectPose, GripStyle gripStyle)
        {
            Vector3 gripCenter = objectGrip.GetGripCenter(gripStyle, objectPose);
            Vector4 palmPlaneNormal = objectGrip.GetPalmPlane(gripStyle, objectPose);
            Vector3 gripAxis = objectGrip.GetGripAxis(gripStyle, objectPose);

            GripState grip = new GripState(gripStyle, gripCenter, palmPlaneNormal, gripAxis);
            return grip;
        }

        /// <summary>
        /// Append an (ordered) list of move and grasp step descriptions for a pick task from an unordered collection. The ordered list of steps is
        /// Start, PreGripLift, PreGrip, Grip, CloseGripper, GripLift and Retract
        /// </summary>
        /// <param name="stepDescription">List to append</param>
        /// <param name="stepDict">Input collection of arm / gripper configurations</param>
        /// <param name="manipulatedObject">Object to be picked</param>
        /// <param name="pickPlaceEnum">Pick and place status to report at each step</param>
        /// <param name="addEndState">When true append a task end state</param>
        private void AppendPickStepDescriptions(
            List<MoveAndGraspTaskDescription> stepDescription, 
            Dictionary<SubTaskTypesEnum, PoseAndGrip> stepDict,
            PhysicalObjectDescription manipulatedObject,
            PicknPlaceStatusEnum pickPlaceEnum = PicknPlaceStatusEnum.PickInProgress,
            bool addEndState = true)
        {
            double closeGripPercent = this.GetGripperOpeningForClose(manipulatedObject, stepDict[SubTaskTypesEnum.Grip].GripperStyle);
            double openGripPercent = this.GetGripperOpeningForOpen(manipulatedObject, stepDict[SubTaskTypesEnum.Grip].GripperStyle);

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Current].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Current].GripperStyle),
                        "Pick - Adjusting orientation",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.PreGripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.PreGripLift].GripperStyle),
                        "Pick - Moving to pre grip lift",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.PreGrip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.PreGrip].GripperStyle),
                        "Pick - Moving to pre grip",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Pick - Moving grasp",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Pick - Closing gripper",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Pick - Lifting from grasp",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Retract].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Retract].GripperStyle),
                        "Pick - retracting arm",
                        this.defaultTimeOutmSec));

            if (true == addEndState)
            {
                stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Retract].MovePose,
                        PicknPlaceStatusEnum.PickComplete,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Retract].GripperStyle),
                        "Pick - Task complete",
                        this.defaultTimeOutmSec));
            }
        }

        /// <summary>
        /// Append an (ordered) list of move and grasp step descriptions for a place task from an unordered collection. The ordered list of steps is
        /// Start, PlaceLift, Place, OpenGripper, PreGrip, PreGripLift, Retract
        /// </summary>
        /// <param name="stepDescription">List to append</param>
        /// <param name="stepDict">Input collection of arm / gripper configurations</param>
        /// <param name="manipulatedObject">Object to be placed</param>
        /// <param name="pickPlaceEnum">Pick and place status to report at each step</param>
        /// <param name="addEndState">When true append a task end state</param>
        private void AppendPlaceStepDescriptions(
            List<MoveAndGraspTaskDescription> stepDescription, 
            Dictionary<SubTaskTypesEnum, PoseAndGrip> stepDict,
            PhysicalObjectDescription manipulatedObject,
            PicknPlaceStatusEnum pickPlaceEnum = PicknPlaceStatusEnum.PlaceInProgress,
            bool addEndState = true)
        {
            double closeGripPercent = this.GetGripperOpeningForClose(manipulatedObject, stepDict[SubTaskTypesEnum.Grip].GripperStyle);
            double openGripPercent = this.GetGripperOpeningForOpen(manipulatedObject, stepDict[SubTaskTypesEnum.Grip].GripperStyle);

            stepDescription.Add(new MoveAndGraspTaskDescription(
                stepDict[SubTaskTypesEnum.Current].MovePose,
                pickPlaceEnum,
                new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Current].GripperStyle),
                "Place - Adjusting orientation",
                this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Place - Moving release lift location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Place - Moving to release location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Place - Open gripper",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.PreGrip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.PreGrip].GripperStyle),
                        "Place - Moving to pre release location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.PreGripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.PreGripLift].GripperStyle),
                        "Place - Moving to pre release lift location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        stepDict[SubTaskTypesEnum.Retract].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Retract].GripperStyle),
                        "Place - Moving to retract",
                        this.defaultTimeOutmSec));

            if (true == addEndState)
            {
                stepDescription.Add(new MoveAndGraspTaskDescription(
                            stepDict[SubTaskTypesEnum.Retract].MovePose,
                            PicknPlaceStatusEnum.PlaceComplete,
                            new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, stepDict[SubTaskTypesEnum.Retract].GripperStyle),
                            "Place - task complete",
                            this.defaultTimeOutmSec));
            }
        }

        /// <summary>
        /// Append an (ordered) list of move and grasp step descriptions for a grip exchange. 
        /// This is an abbreviated set pick and place steps because we know there are no obstructions at the 
        /// grip exchange. The list is 
        /// Place at grip exchange Location: Start, GripLift, Grip, GripLift
        /// Pick at grip Exchange: GripLift, Grip, GripLift, retract
        /// </summary>
        /// <param name="stepDescription">List to append</param>
        /// <param name="placeDict">Input collection of arm / gripper configurations for place</param>
        /// <param name="pickDict">Input collection of arm / gripper configurations for pick</param>
        /// <param name="manipulatedObject">Object to be exchanged</param>
        private void AppendGripExchangeStepDescriptions(
            List<MoveAndGraspTaskDescription> stepDescription,
            Dictionary<SubTaskTypesEnum, PoseAndGrip> placeDict,
            Dictionary<SubTaskTypesEnum, PoseAndGrip> pickDict,
            PhysicalObjectDescription manipulatedObject)
        {
            PicknPlaceStatusEnum pickPlaceEnum = PicknPlaceStatusEnum.PlaceInProgress;
            double closeGripPercent = this.GetGripperOpeningForClose(manipulatedObject, placeDict[SubTaskTypesEnum.Grip].GripperStyle);
            double openGripPercent = this.GetGripperOpeningForOpen(manipulatedObject, placeDict[SubTaskTypesEnum.Grip].GripperStyle);

            stepDescription.Add(new MoveAndGraspTaskDescription(
                placeDict[SubTaskTypesEnum.Current].MovePose,
                pickPlaceEnum,
                new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, placeDict[SubTaskTypesEnum.Current].GripperStyle),
                "Place - Adjusting orientation",
                this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        placeDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, placeDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Place - Moving release lift location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        placeDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, placeDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Place - Moving to release location",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        placeDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, placeDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Place - Open gripper",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        placeDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, placeDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Place - Lifting from grasp",
                        this.defaultTimeOutmSec));

            // Change to the pick grip
            closeGripPercent = this.GetGripperOpeningForClose(manipulatedObject, pickDict[SubTaskTypesEnum.Grip].GripperStyle);
            openGripPercent = this.GetGripperOpeningForOpen(manipulatedObject, pickDict[SubTaskTypesEnum.Grip].GripperStyle);

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Pick - Adjusting grasp orientation",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.PreGrip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.PreGrip].GripperStyle),
                        "Pick - Moving to pre grip",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Open, openGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Pick - Moving grasp",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.Grip].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.Grip].GripperStyle),
                        "Pick - Closing gripper",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.GripLift].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.GripLift].GripperStyle),
                        "Pick - Lifting from grasp",
                        this.defaultTimeOutmSec));

            stepDescription.Add(new MoveAndGraspTaskDescription(
                        pickDict[SubTaskTypesEnum.Retract].MovePose,
                        pickPlaceEnum,
                        new GripperStatus(GripperStatus.GripperStateEnum.Close, closeGripPercent, GripperStatus.FingerStateEnum.Unknown, pickDict[SubTaskTypesEnum.Retract].GripperStyle),
                        "Pick - retracting arm",
                        this.defaultTimeOutmSec));
        }

        /// <summary>
        /// Gets the suggested opening percentage of the gripper to close on the object for the grip style
        /// </summary>
        /// <param name="manipulatedObject">The manipulated object</param>
        /// <param name="gripstyle">Current grip style</param>
        /// <returns>Opening expressed as percentage for current gripper</returns>
        private double GetGripperOpeningForClose(PhysicalObjectDescription manipulatedObject, GripStyle gripstyle)
        {
            IPicknPlaceObjectGrip objectGrip = PicknPlaceObjectGrip.ObjectGripFactory(manipulatedObject);
            double gripDimensionInM = objectGrip.GetGripperOpeningInM(gripstyle);
            double gripOpeningPercent = this.armHandGripper.GetPercentOpenForCloseState(gripDimensionInM);
            return gripOpeningPercent;
        }

        /// <summary>
        /// Gets the suggested opening percentage of the gripper to close on the object for the grip style
        /// </summary>
        /// <param name="manipulatedObject">The manipulated object</param>
        /// <param name="gripstyle">Current grip style</param>
        /// <returns>Opening expressed as percentage for current gripper</returns>
        private double GetGripperOpeningForOpen(PhysicalObjectDescription manipulatedObject, GripStyle gripstyle)
        {
            IPicknPlaceObjectGrip objectGrip = PicknPlaceObjectGrip.ObjectGripFactory(manipulatedObject);
            double gripDimensionInM = objectGrip.GetGripperOpeningInM(gripstyle);
            double gripOpeningPercent = this.armHandGripper.GetPercentOpenForOpenState(gripDimensionInM);
            return gripOpeningPercent;
        }

        /// <summary>
        /// Returns a collection of arm  / gripper configurations for all steps in a pick or place sub task indexed by 
        /// step type. The first grip style that satisfies all steps of the task is chosen
        /// </summary>
        /// <param name="manipulatedObject">Manipulated object</param>
        /// <param name="startPose">Position at start of the task. Enables re-orientation before a move</param>
        /// <param name="objectPose">Pose of the object to be grasped (example pick or place location)</param>
        /// <param name="retractPose">Pose of object when grasped (example move to location or move from location)</param>
        /// <param name="liftHeight">Height object should be lifted in the pick action</param>
        /// <returns>Set of configurations for task steps</returns>
        /// <exception cref="NoGripAvailableException">Thrown when no grip is available for the task</exception>
        private Dictionary<SubTaskTypesEnum, PoseAndGrip> GetStepDescriptionsForAnyGrip(
            PhysicalObjectDescription manipulatedObject,
            Pose startPose,
            Pose objectPose,
            Pose retractPose,
            double liftHeight)
        {
            bool foundGrip = false;
            IPicknPlaceObjectGrip objectGrip = PicknPlaceObjectGrip.ObjectGripFactory(manipulatedObject);

            double surfaceOffset = objectGrip.GetHorizontalPlaneOffset(objectPose);
            List<Tuple<GripState, GripState, Pose, Pose, double>> gripScores = new List<Tuple<GripState, GripState, Pose, Pose, double>>();
            Vector3 liftAdd = new Vector3(0, 0, 1) * liftHeight;

            foreach (GripStyle gripStyle in objectGrip)
            {
                try
                {
                    GripState gripCand = this.GripFromPickAndGripStyle(objectGrip, objectPose, gripStyle);
                    GripState preGripCand = this.PreGripFromGrip(gripCand);

                    double worstScore = 0;
                    Pose preGraspCand = this.ArmPoseFromGrip(preGripCand, surfaceOffset, out worstScore);
                    double score;
                    Pose graspCand = this.ArmPoseFromGrip(gripCand, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);

                    // check the Lift positions
                    GripState gripLift = new GripState(gripCand.GripStyle, gripCand.GripCenterGlobalLocation + liftAdd, gripCand.PalmPlane, gripCand.GripCenterAxis);
                    this.ArmPoseFromGrip(gripLift, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);
                    GripState preGripLift = new GripState(preGripCand.GripStyle, preGripCand.GripCenterGlobalLocation + liftAdd, preGripCand.PalmPlane, preGripCand.GripCenterAxis);
                    this.ArmPoseFromGrip(preGripLift, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);

                    gripScores.Add(new Tuple<GripState, GripState, Pose, Pose, double>(gripCand, preGripCand, preGraspCand, graspCand, worstScore));
                    foundGrip = true;
                }
                catch (ArgumentException)
                {
                    // Try another grip
                }
            }

            if (false == foundGrip)
            {
                throw new NoGripAvailableException(string.Format("Cannot find a suitable grip for {0} at location {1} {2} {3}", objectGrip.ToString(), objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
            }

            gripScores.Sort((a, b) => { return -a.Item5.CompareTo(b.Item5); });
            GripState grip = gripScores[0].Item1;
            GripState preGrip = gripScores[0].Item2;
            Pose preGrasp = gripScores[0].Item3;
            Pose grasp = gripScores[0].Item4;

            Vector3 liftGripPosition = grasp.Position + liftAdd;
            Vector3 liftPreGripPosition = new Vector3(preGrasp.Position.X, preGrasp.Position.Y, liftGripPosition.Z);
            Pose graspLift = new Pose(liftGripPosition, grasp.Orientation);
            Pose preGraspLift = new Pose(liftPreGripPosition, preGrasp.Orientation);

            Dictionary<SubTaskTypesEnum, PoseAndGrip> dict = new Dictionary<SubTaskTypesEnum, PoseAndGrip>();
            dict.Add(SubTaskTypesEnum.PreGripLift, new PoseAndGrip(preGraspLift, preGrip.GripStyle));
            dict.Add(SubTaskTypesEnum.PreGrip, new PoseAndGrip(preGrasp, preGrip.GripStyle));
            dict.Add(SubTaskTypesEnum.Grip, new PoseAndGrip(grasp, grip.GripStyle));
            dict.Add(SubTaskTypesEnum.GripLift, new PoseAndGrip(graspLift, grip.GripStyle));

            this.AddStartRetract(objectGrip, startPose, retractPose, grip.GripStyle, surfaceOffset, dict);

            Tracing.TraceOut.Info(TraceContexts.Manip, "Using gripstyle {0}", grip.GripStyle.ToString());
            return dict;
        }

        /// <summary>
        /// Returns a set of arm  / gripper configurations for all steps in a pick or place sub task indexed by the
        /// step type for a specific grip style
        /// </summary>
        /// <param name="manipulatedObject">Manipulated object</param>
        /// <param name="gripStyleRequest">Specified grip style</param>
        /// <param name="startPose">Position at start of the task. Enables re-orientation before a move</param>
        /// <param name="objectPose">Pose of the object to be grasped (example pick or place location)</param>
        /// <param name="retractPose">Pose of object when grasped (example move to location or move from location)</param>
        /// <param name="liftHeight">Height object should be lifted in the pick action</param>
        /// <returns>Dictionary of subtask parameters</returns>
        /// <exception cref="ArgumentException">If the task cannot be achieved with requested grip style</exception>
        private Dictionary<SubTaskTypesEnum, PoseAndGrip> GetStepDescriptionsForGrip(
            PhysicalObjectDescription manipulatedObject,
            GripStyle gripStyleRequest,
            Pose startPose,
            Pose objectPose,
            Pose retractPose,
            double liftHeight)
        {
            if (gripStyleRequest == GripStyle.Unknown)
            {
                return this.GetStepDescriptionsForAnyGrip(manipulatedObject, startPose, objectPose, retractPose, liftHeight);
            }

            IPicknPlaceObjectGrip objectGrip = PicknPlaceObjectGrip.ObjectGripFactory(manipulatedObject);

            double surfaceOffset = objectGrip.GetHorizontalPlaneOffset(objectPose);
            Vector3 liftAdd = new Vector3(0, 0, 1) * liftHeight;

            bool foundGrip = false;
            List<Tuple<GripState, GripState, Pose, Pose, double>> gripScores = new List<Tuple<GripState, GripState, Pose, Pose, double>>();

            foreach (GripStyle gripStyle in objectGrip.GetEquivalentGripStyleSet(gripStyleRequest))
            {
                try
                {
                    GripState gripCand = this.GripFromPickAndGripStyle(objectGrip, objectPose, gripStyle);
                    GripState preGripCand = this.PreGripFromGrip(gripCand);

                    double worstScore;
                    Pose preGraspCand = this.ArmPoseFromGrip(preGripCand, surfaceOffset, out worstScore);
                    double score;
                    Pose graspCand = this.ArmPoseFromGrip(gripCand, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);

                    // check the Lift positions
                    GripState gripLift = new GripState(gripCand.GripStyle, gripCand.GripCenterGlobalLocation + liftAdd, gripCand.PalmPlane, gripCand.GripCenterAxis);
                    this.ArmPoseFromGrip(gripLift, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);
                    GripState preGripLift = new GripState(preGripCand.GripStyle, preGripCand.GripCenterGlobalLocation + liftAdd, preGripCand.PalmPlane, preGripCand.GripCenterAxis);
                    this.ArmPoseFromGrip(preGripLift, surfaceOffset, out score);
                    worstScore = Math.Min(score, worstScore);

                    gripScores.Add(new Tuple<GripState, GripState, Pose, Pose, double>(gripCand, preGripCand, preGraspCand, graspCand, worstScore));

                    foundGrip = true;
                    break;
                }
                catch (ArgumentException)
                {
                    // Try another grip
                    foundGrip = false;
                }
            }

            if (false == foundGrip)
            {
                throw new NoGripAvailableException(string.Format("Cannot find a suitable grip for {0} at location {1} {2} {3}", objectGrip.ToString(), objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
            }

            gripScores.Sort((a, b) => { return -a.Item5.CompareTo(b.Item5); });
            GripState grip = gripScores[0].Item1;
            GripState preGrip = gripScores[0].Item2;
            Pose preGrasp = gripScores[0].Item3;
            Pose grasp = gripScores[0].Item4;

            Vector3 liftGripPosition = grasp.Position + liftAdd;
            Vector3 liftPreGripPosition = new Vector3(preGrasp.Position.X, preGrasp.Position.Y, liftGripPosition.Z);
            Pose graspLift = new Pose(liftGripPosition, grasp.Orientation);
            Pose preGraspLift = new Pose(liftPreGripPosition, preGrasp.Orientation);

            Dictionary<SubTaskTypesEnum, PoseAndGrip> dict = new Dictionary<SubTaskTypesEnum, PoseAndGrip>();
            dict.Add(SubTaskTypesEnum.PreGripLift, new PoseAndGrip(preGraspLift, preGrip.GripStyle));
            dict.Add(SubTaskTypesEnum.PreGrip, new PoseAndGrip(preGrasp, preGrip.GripStyle));
            dict.Add(SubTaskTypesEnum.Grip, new PoseAndGrip(grasp, grip.GripStyle));
            dict.Add(SubTaskTypesEnum.GripLift, new PoseAndGrip(graspLift, grip.GripStyle));
            this.AddStartRetract(objectGrip, startPose, retractPose, grip.GripStyle, surfaceOffset, dict);

            return dict;
        }

        /// <summary>
        /// Adds start and retract poses to the dictionary of sub tasks, that respects 
        /// the requested grip style
        /// </summary>
        /// <param name="objectGrip">Object grip instance to use</param>
        /// <param name="startPose">Start pose of object</param>
        /// <param name="retractPose">Retract pose</param>
        /// <param name="gripStyleRequest">Requested grip style</param>
        /// <param name="surfaceOffset">Offset of the surface</param>
        /// <param name="dict">Sub task dictionary</param>
        private void AddStartRetract(IPicknPlaceObjectGrip objectGrip, Pose startPose, Pose retractPose, GripStyle gripStyleRequest, double surfaceOffset, Dictionary<SubTaskTypesEnum, PoseAndGrip> dict)
        {
            GripState gripRetract;
            Pose retractPoseManip;
            List<Tuple<GripState, Pose, double>> gripScores = new List<Tuple<GripState, Pose, double>>();
            double score;

            foreach (GripStyle gripStyle in objectGrip.GetEquivalentGripStyleSet(gripStyleRequest))
            {
                try
                {
                    gripRetract = this.GripFromPickAndGripStyle(objectGrip, retractPose, gripStyle);
                    retractPoseManip = this.ArmPoseFromGrip(gripRetract, surfaceOffset, out score);
                    gripScores.Add(new Tuple<GripState, Pose, double>(gripRetract, retractPoseManip, score));
                }
                catch (ArgumentException)
                {
                    // try another one
                }
            }

            if (gripScores.Count <= 0)
            {
                throw new NoGripAvailableException(string.Format("Cannot retract with Grip {0} to location {1} {2} {3}", gripStyleRequest.ToString(), retractPose.Position.X, retractPose.Position.Y, retractPose.Position.Z));
            }

            gripScores.Sort((a, b) => { return -a.Item3.CompareTo(b.Item3); });
            gripRetract = gripScores[0].Item1;
            retractPoseManip = gripScores[0].Item2;

            GripState startGrip = this.GripFromPickAndGripStyle(objectGrip, startPose, gripRetract.GripStyle);
            Pose currentAdjPose = this.ArmPoseFromGrip(startGrip, surfaceOffset, out score);
            dict.Add(SubTaskTypesEnum.Current, new PoseAndGrip(currentAdjPose, gripRetract.GripStyle));
            dict.Add(SubTaskTypesEnum.Retract, new PoseAndGrip(retractPoseManip, gripRetract.GripStyle));
        }

        /// <summary>
        /// Return a parallel palm plane but offset by an amount
        /// </summary>
        /// <param name="gripPalmNormal">Grasping plane</param>
        /// <param name="preGripOffset">Offset for pre Grip from the Grasping plane</param>
        /// <returns>Pre grip plane</returns>
        private Vector4 OffsetPalmPlane(Vector4 gripPalmNormal, double preGripOffset)
        {
            Vector4 preGrip = new Vector4(gripPalmNormal);
            preGrip.W = preGrip.W + preGripOffset;

            return preGrip;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="PoseAndGrip" /> class
        /// This is a convenience class to collect an arm pose and gripper style
        /// </summary>
        internal class PoseAndGrip
        {
            /// <summary>
            /// Initializes a new instance of the <see cref="PoseAndGrip" /> class
            /// </summary>
            /// <param name="pose">Arm pose</param>
            /// <param name="style">Gripper style</param>
            internal PoseAndGrip(Pose pose, GripStyle style)
            {
                this.MovePose = pose;
                this.GripperStyle = style;
            }

            /// <summary>
            /// Gets the pose
            /// </summary>
            internal Pose MovePose { get; private set; }

            /// <summary>
            /// Gets the grip style
            /// </summary>
            internal GripStyle GripperStyle { get; private set; }
        }
    }
}
